#include <math.h>

#include "motor_ctrl.h"

Motor_YBT::Motor_YBT(Motor_ID_Info &info) : Motor(info)
{
    std::cout<<"Motor_YBT"<<std::endl;
    this->accept_err        = 0.5f;                //YBT的精度差一点
    this->each_angle_max    = 5.0f;
    this->cmd_freq          = YBT_FREQ;              //YBT最快500Hz
    this->dir               = DIR_CLOCKWISE;                
}

Motor_YBT::~Motor_YBT()
{
    std::cout<<"~Motor_YBT"<<std::endl;
}

/* Converts a float to an unsigned int, given range and number of bits */
static int32_t float_to_uint(double x, double x_min, double x_max, uint8_t bits)
{
    double span = x_max - x_min;
    if(x < x_min) {
        x = x_min;
    } else if(x > x_max) {
        x = x_max;
    } else {
        ;
    }
    return (int32_t) ((x- x_min)*((double)(((1<<bits)-1)/span)));
}

double uint_to_float(int32_t x_int, double x_min, double x_max, uint8_t bits)
{
    double span = x_max - x_min;
    double offset = x_min;
    return (((uint16_t)x_int)*span)/((double)(((1<<bits)-1))) + offset; //对于pos 务必使用(uint16_t)
}

int32_t Motor_YBT:: set_origin(uint8_t mode)
{
    return 0;
}
                                       
int32_t Motor_YBT:: set_motor_pos(double pos)
{
#define P_MIN (-720)
#define P_MAX (720)
#define P_BITS (16)
    if((pos >= P_MIN) && (pos <= P_MAX)) {
        this->pos = pos;
        return float_to_uint(pos, P_MIN, P_MAX, P_BITS);
    } else {
        return float_to_uint(0, P_MIN, P_MAX, P_BITS); 
    }
}

int32_t Motor_YBT:: set_motor_w(double w)
{
#define V_MIN   (-30.0f*180/3.14)
#define V_MAX   (30.0f*180/3.14)
#define V_BITS  (12)
    if((w >= V_MIN) && (w <= V_MAX)) {
        this->w = w;
        return float_to_uint(w*180/3.14, V_MIN, V_MAX, V_BITS);
    } else {
        return float_to_uint(0, V_MIN, V_MAX, V_BITS); 
    }
} 

int32_t Motor_YBT:: set_motor_t(double t)
{
#define T_MIN (-48)
#define T_MAX (48)
#define T_BITS (12)
    if((t >= V_MIN) && (t <= V_MAX)) {
        this->t = t;
        return float_to_uint(t, T_MIN, T_MAX, T_BITS);
    } else {
        return float_to_uint(0, T_MIN, T_MAX, T_BITS); 
    }
       
}

int32_t Motor_YBT:: set_motor_acc(double acc)
{
    return 0;
}

int32_t Motor_YBT:: set_motor_kp(double kp)
{
#define KP_MIN (0)
#define KP_MAX (500)
#define KP_BITS (12)
    if((kp >= KP_MIN) && (kp <= V_MAX)) {
        this->kp = kp;
        return float_to_uint(kp, KP_MIN, KP_MAX, KP_BITS);
    } else {
        return float_to_uint(0, KP_MIN, KP_MAX, KP_BITS); 
    }
} 

int32_t Motor_YBT:: set_motor_kd(double kd)
{
#define KD_MIN (0)
#define KD_MAX (100)
#define KD_BITS (12)
    if((kd >= KD_MIN) && (kd <= KD_MAX)) {
        this->kd = kd;
        return float_to_uint(kd, KD_MIN, KD_MAX, KD_BITS);
    } else {
        return float_to_uint(0, KD_MIN, KD_MAX, KD_BITS); 
    }
}

void Motor_YBT::set_cur_pos(uint8_t data[])
{
#define P_MIN (-720)
#define P_MAX (720)
#define P_BITS (16)
    int16_t pos = ((data[0] << 8) | data[1]); 
    this->cur_pos = uint_to_float(pos, P_MIN, P_MAX, P_BITS);      
}

void Motor_YBT::set_cur_w(uint8_t data[])
{
 
}

void Motor_YBT::set_cur_tau(uint8_t data[])
{

}

int32_t Motor_YBT::set_motor_dataframe(uint8_t dataframe[], Motor_Mode mode, double value[], uint8_t len)
{
    int32_t data_len = 8;
    int32_t val;
    switch (mode) {
        case MOTOR_MODE_ENTER:
            dataframe[0] = 0xFF;
            dataframe[1] = 0xFF;
            dataframe[2] = 0xFF;
            dataframe[3] = 0xFF;
            dataframe[4] = 0xFF;
            dataframe[5] = 0xFF;
            dataframe[6] = 0xFF;
            dataframe[7] = 0xFC;
            break;
        case MOTOR_MODE_EXIT:
            dataframe[0] = 0xFF;
            dataframe[1] = 0xFF;
            dataframe[2] = 0xFF;
            dataframe[3] = 0xFF;
            dataframe[4] = 0xFF;
            dataframe[5] = 0xFF;
            dataframe[6] = 0xFF;
            dataframe[7] = 0xFD;               
            break;
        case MOTOR_MODE_ORIGIN:
            // dataframe[0] = 0xFF;
            // dataframe[1] = 0xFF;
            // dataframe[2] = 0xFF;
            // dataframe[3] = 0xFF;
            // dataframe[4] = 0xFF;
            // dataframe[5] = 0xFF;
            // dataframe[6] = 0xF1;
            // dataframe[7] = 0x00; 
            // break;
            return 0;   //YBT电机在代码中禁止使用零点设置命令（应使用工具提前设置好零点）
        case MOTOR_MODE_W: //value为角速度
            if(1 > len) {
                cout << "MOTOR_TYPE_YBT MOTOR_MODE_W len error!" << endl;
                return -1;
            }
            val = set_motor_pos(0);
            dataframe[0] = (val >> 8) & 0xff;
            dataframe[1] = val & 0xFF;
            val = set_motor_w(value[0]);
            dataframe[2] = (val >> 4) & 0xFF;
            dataframe[3] = (val << 4) & 0xF0;
            val = set_motor_kp(0);
            dataframe[3] |= (val >> 8) & 0x0F;
            dataframe[4] = val & 0xFF;
            val = set_motor_kd(6.5);
            dataframe[5] = (val >> 4) & 0xFF;
            dataframe[6] = (val << 4) & 0xF0;
            val = set_motor_t(0);
            dataframe[6] |= (val >> 8) & 0x0F;
            dataframe[7] = val & 0xFF;
            break; 
        case MOTOR_MODE_POS_HOLD:   //value为位置
        case MOTOR_MODE_POS_TRACK:
        case MOTOR_MODE_MIX:    //YBT无混合控制，这里将其等价位位置控制
            if(1 > len) {
                cout << "MOTOR_TYPE_YBT MOTOR_MODE_POS len error!" << endl;
                return -1;
            }
            val = set_motor_pos(value[0]);
            dataframe[0] = (val >> 8) & 0xff;
            dataframe[1] = val & 0xFF;
            val = set_motor_w(0);
            dataframe[2] = (val >> 4) & 0xFF;
            dataframe[3] = (val << 4) & 0xF0;
            // val = set_motor_kp(2.0);         
            val = set_motor_kp(79.976f);
            dataframe[3] |= (val >> 8) & 0x0F;
            dataframe[4] = val & 0xFF;
            // val = set_motor_kd(6.5);
            val = set_motor_kd(20.0f);
            dataframe[5] = (val >> 4) & 0xFF;
            dataframe[6] = (val << 4) & 0xF0;
            val = set_motor_t(0);
            dataframe[6] |= (val >> 8) & 0x0F;
            dataframe[7] = val & 0xFF;
            break;
        case MOTOR_MODE_T:  //value为扭矩
            if(1 > len) {
                cout << "MOTOR_TYPE_YBT MOTOR_MODE_T len error!" << endl;
                return -1;
            }
            val = set_motor_pos(0);
            dataframe[0] = (val >> 8) & 0xff;
            dataframe[1] = val & 0xFF;
            val = set_motor_w(0);
            dataframe[2] = (val >> 4) & 0xFF;
            dataframe[3] = (val << 4) & 0xF0;
            val = set_motor_kp(0);
            dataframe[3] |= (val >> 8) & 0x0F;
            dataframe[4] = val & 0xFF;
            val = set_motor_kd(0);
            dataframe[5] = (val >> 4) & 0xFF;
            dataframe[6] = (val << 4) & 0xF0;
            val = set_motor_t(value[0]);
            dataframe[6] |= (val >> 8) & 0x0F;
            dataframe[7] = val & 0xFF;
            break;
        default:
            break;
    }
#if 0
    printf("can_send:");
    for(uint8_t i =0;i<8;i++) {
        printf("%02x ",dataframe[i]);
    }
    printf("\n");
#endif
    return data_len;
}